#颜色检测类
class ColorDetectClass():
    def __init__(self):
        self.colorList = [] #颜色列表
        self.objectShape = {} #目标形状
        self.VvalueLow = None #避免出错，初始化为None

    #在列表中找到最大的 num2find 个值，并返回其索引列表
    def findNBiggestFromList(self, areaList, num2find):
        areaListCopy = copy.copy(areaList)
        indexList = []
        for i in range(num2find):
            amax = max(areaListCopy)
            aindex = areaListCopy.index(amax)
            areaListCopy[aindex] = -1
            indexList.append(aindex)
        return indexList

    #返回一个颜色列表包含num个颜色
    def colorListGenerate(self, num):
        if num < 0 or not isinstance(num, int):
            print('check your input value qqddff', num)
            return []
        if num == 0:
            return
        if num > 100: #不能太大
            num = 100
        down1 = 100
        up1 = 255
        down2 = 0
        up2 = 100
        R = list(range(down1, up1, int((up1-down1)/num)))
        G = list(range(up2, down2, -int((up2-down2)/num)))
        B = [random.randint(150,255) for x in range(num)]
        self.colorList = [(R[i], G[i], B[i]) for i in range(num)]
        return self.colorList

    #生成需要检测的目标轮廓 #没有用，不用管
    def aimContoursGenerate(self):
        #形状 - 圆
        img1 = cv.imread('object/aim_circle.png',0)
        ret,shape1 = cv.threshold(img1,127,255,cv.THRESH_BINARY)
        imguseless, contours, hierarchy = cv.findContours(shape1, cv.RETR_LIST , cv.CHAIN_APPROX_SIMPLE) 
        self.objectShape['circle'] = contours[0] #会检测出两个轮廓，取第一个
        #形状 - 90度扇形
        img1 = cv.imread('object/aim_sector90.png',0)
        ret,shape1 = cv.threshold(img1,127,255,cv.THRESH_BINARY)
        imguseless, contours, hierarchy = cv.findContours(shape1, cv.RETR_LIST , cv.CHAIN_APPROX_SIMPLE) 
        self.objectShape['sector90'] = contours[0]
        #形状 - 长宽比1:1.5矩形
        img1 = cv.imread('object/aim_rect1X1.5.png',0)
        ret,shape1 = cv.threshold(img1,127,255,cv.THRESH_BINARY)
        imguseless, contours, hierarchy = cv.findContours(shape1, cv.RETR_LIST , cv.CHAIN_APPROX_SIMPLE) 
        self.objectShape['rect1X1.5'] = contours[0]
        #形状 - 长宽比1:3矩形
        img1 = cv.imread('object/aim_rect1X3.png',0)
        ret,shape1 = cv.threshold(img1,127,255,cv.THRESH_BINARY)
        imguseless, contours, hierarchy = cv.findContours(shape1, cv.RETR_LIST , cv.CHAIN_APPROX_SIMPLE) 
        self.objectShape['rect1X3'] = contours[0]

        #调试：
        # img1rgb = cv.cvtColor(img1, cv.COLOR_GRAY2BGR)
        # print(len(contours))
        # self.colorList = self.colorListGenerate(5) #生成一个颜色列表
        # for i, cnt in enumerate(contours):
        #     M = cv.moments(cnt) #计算特征矩
        #     area = M['m00'] #表示轮廓的面积     
        #     if area > 10: #面积不能太小
        #         print('kkk:',area)
        #         cv.drawContours(img1rgb, cnt, -1, self.colorList[i], 6)
        # cv.imshow('shape1',img1rgb)
        # cv.waitKey(0)
        # exit()

    #检测图像中的颜色 which='greenW' 'yellowCar'
    def detect(self, imgCameraRGB, which):
        hsv = cv.cvtColor(imgCameraRGB, cv.COLOR_BGR2HSV) # 转换颜色空间 BGR 到 HSV
        if which == 'greenW':     
            Hvalue = 52 #55 # 定义HSV颜色范围 绿色的W背景
            Htolerance = 5
            areaLeastValue = 600 #设置阈值!
        elif which == 'yellowCar':
            Hvalue = 35 # 定义HSV颜色范围 黄色的Car车窗
            Htolerance = 5
            areaLeastValue = 100 #设置阈值!
        else:
            print('can not be here! yynnzz')
        if not self.VvalueLow is None: #如果存在self.Vvalue(调试用)
            lower_color = np.array([self.Hvalue-self.Htolerance, self.SvalueLow, self.VvalueLow])
            upper_color = np.array([self.Hvalue+self.Htolerance, self.SvalueUp, self.VvalueUp])  
        else:
            lower_color = np.array([Hvalue-Htolerance,50,50])
            upper_color = np.array([Hvalue+Htolerance,255,255])      
        mask = cv.inRange(hsv, lower_color, upper_color) # 设置HSV的阈值使得只取特定颜色
        # #利用开运算滤除小点，(即先侵蚀，再扩张)
        # kernel = np.ones((5,5),np.uint8) #内核
        # opening = cv.morphologyEx(mask, cv.MORPH_OPEN, kernel) 

        #opencv不同版本的 findContours 的参数有区别，看官方手册时注意选择对应的版本!
        imguseless, contours, hierarchy = cv.findContours(mask, cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE) 
        areaList = [] #记录每个contours的面积大小
        centroidList = [] #记录每个contours形心位置
        contoursList = []  #记录每个有效的contours
        for i, cnt in enumerate(contours):
            M = cv.moments(cnt) #计算特征矩
            area = M['m00'] #表示轮廓的面积   
            if area > areaLeastValue: #面积不能太小
                cx = int(M['m10']/M['m00'])
                cy = int(M['m01']/M['m00'])
                areaList.append(area)
                centroidList.append((cx,cy))
                contoursList.append(cnt)

        self.colorList = self.colorListGenerate(len(areaList)) #生成一个颜色列表
        colorIndex = 0
        MassX = MassY = 10000 #记录质心的坐标 #初始化为很大的数  
        if which == 'greenW':
            if len(areaList) >= 1: #至少需要找到1个
                maxIndex = self.findNBiggestFromList(areaList, 1) #找到最大值所在的索引
                #画出质心
                MassX = centroidList[maxIndex[0]][0]
                MassY = centroidList[maxIndex[0]][1]
                MassX = mProcess.lowpass1p('MassX',MassX) #低通滤波
                MassY = mProcess.lowpass1p('MassY',MassY)
                cv.circle(imgCameraRGB, (int(MassX),int(MassY)), 8, [255,0,200], -1)
                #print('center:', centroidList[maxIndex[0]][0],centroidList[maxIndex[0]][1])
        elif which == 'yellowCar':
            if len(areaList) >= 2: #至少需要找到2个
                max3Index = self.findNBiggestFromList(areaList, 3) #找到3个最大值所在的索引

                #首先进行一些判断，排除掉不合理的情况
                fitnessFlag = 0 
                judging1 = (areaList[max3Index[0]] - areaList[max3Index[1]]) / areaList[max3Index[0]] 
                if judging1 > 0.4: #两个轮廓的面积差不能太大
                    fitnessFlag = 1
                #length: 两个轮廓质心之间的距离
                length = math.sqrt((centroidList[max3Index[0]][0] - centroidList[max3Index[1]][0])**2 + (centroidList[max3Index[0]][1] - centroidList[max3Index[1]][1])**2)
                length_stic = math.sqrt(areaList[max3Index[0]]) #轮廓的特征长度                
                if length > length_stic*3: #两个轮廓之间的距离不能太大
                    fitnessFlag = 2
                judging3 = 0
                # if len(areaList) >= 3:
                #     judging3 = (areaList[max3Index[1]] - areaList[max3Index[2]]) / areaList[max3Index[1]]
                #     if  judging3 < 0.2: #第3个轮廓的面积需要远小于第2个轮廓
                #         fitnessFlag = 3
                if len(contoursList[max3Index[0]]) < 5 or len(contoursList[max3Index[1]]) < 5: #至少需要5个点才能拟合椭圆
                    fitnessFlag = 4

                #如果轮廓合理则找中心
                if fitnessFlag == 0:
                    #cv.drawContours(imgCameraRGB, contoursList[max3Index[0], -1, [0,255,0], 4) #画轮廓
                    #cv.drawContours(imgCameraRGB, contoursList[max3Index[1], -1, [255,0,0], 4)
                    (x1,y1),(MA1,ma1),angle1 = cv.fitEllipse(contoursList[max3Index[0]]) #拟合椭圆
                    (x2,y2),(MA2,ma2),angle2 = cv.fitEllipse(contoursList[max3Index[1]])
                    #现在找到了两个轮廓，下一步需要找到小车的中心在哪里，其中心就在两个轮廓质心连线的中垂线方向
                    angle1 -= 90 #opencv是以图像左上角为原点(向右为0角度)，而fitEllipse函数得出的angle是以向上为0角度(逆时针为正)，所以角度要-90
                    angle2 -= 90
                    if abs(angle1 - angle2) > 180:
                        DirMidVertical = (angle1+angle2)/2 + 180 #中垂线方向
                    else:
                        DirMidVertical = (angle1+angle2)/2
                    DirMidVertical = DirMidVertical*math.pi/180 #度转弧度
                    #两个轮廓质心连线的中点坐标
                    midPointX = ( centroidList[max3Index[0]][0] + centroidList[max3Index[1]][0] )/2 
                    midPointY = ( centroidList[max3Index[0]][1] + centroidList[max3Index[1]][1] )/2
                    length *= 0.65 #比例差不多这么多
                    MassX = midPointX + length*math.cos(DirMidVertical)
                    MassY = midPointY + length*math.sin(DirMidVertical)
                    MassX = mProcess.lowpass1p('MassX',MassX) #低通滤波
                    MassY = mProcess.lowpass1p('MassY',MassY)
                    #画出中心
                    cv.circle(imgCameraRGB,(int(MassX),int(MassY)), 8, [0,200,255], -1)
                else:
                    pass
                    #print('contour is not car! %d %.3f %.3f %.3f %.3f ' % (fitnessFlag, judging1 ,judging3, length, length_stic*3), end='')
                    #print('contour is not car!')
        else:
            print('can not be here! yynnzz')

        #print('find', len(contours), 'contours', ' valid area:',len(areaList))
        h, w, channel = imgCameraRGB.shape
        BiasX = MassX - w/2 #计算与中心偏差的像素
        BiasY = MassY - h/2
        BiasW = 0 #偏差的角度
        return imgCameraRGB, mask, BiasX, BiasY, BiasW
